#include <fstream>
#include <iomanip>
#include "ros/Path.h"
#include "dpptam/EurocReader.h"
#include "opencv2/core/core.hpp"
#include <string>
#include <dpptam/superpixel.h>
#include <dpptam/DenseMapping.h>
#include <dpptam/SemiDenseTracking.h>
#include <dpptam/SemiDenseMapping.h>

using namespace std;

int main(int argc, char **argv) {

    cv::FileStorage fs2((ros::package::getPath("dpptam") + "/src/data.yml").c_str(), cv::FileStorage::READ);

    ///vo_system launch the three threads, tracking, semidense mapping and dense mapping (3D superpixels)
    DenseMapping dense_mapper(fs2);
    SemiDenseTracking semidense_tracker(fs2);
    SemiDenseMapping semidense_mapper(fs2);
    MapShared Map;
    Imagenes images, images_previous_keyframe;

    int cont_frames;
    double stamps;
    cv::Mat image_frame, image_frame_aux;
    double depth_stamps;


    std::string camera_path;
    fs2["camera_path"] >> camera_path;
    cont_frames = 0;

    int calculate_superpixels = (int) fs2["calculate_superpixels"];

    vector<string> vstrImageLeft;
    vector<string> vstrImageRight;
    vector<double> vTimeStamp;


//    cv::FileStorage fs2(configFile, cv::FileStorage::READ);

    if (!fs2.isOpened()) {
        cout << "ERROR: Wrong path to settings" << endl;
        return -1;
    }
    string leftFolder = fs2["LeftFolder"];
    string rightFolder = fs2["RightFolder"];
    string timeFolder = fs2["TimeFolder"];
    string imuFolder = fs2["IMUFolder"];

    LoadImages(leftFolder, rightFolder, timeFolder, vstrImageLeft, vstrImageRight, vTimeStamp);

    string vocFile = fs2["VOCFolder"];


    if (vstrImageLeft.empty() || vstrImageRight.empty()) {
        cerr << "ERROR: No images in provided path." << endl;
        return 1;
    }

    if (vstrImageLeft.size() != vstrImageRight.size()) {
        cerr << "ERROR: Different number of left and right images." << endl;
        return 1;
    }

    // Read rectification parameters
//    cv::FileStorage fs2(configFile.cs, cv::FileStorage::READ);
    if (!fs2.isOpened()) {
        cerr << "ERROR: Wrong path to settings" << endl;
        return -1;
    }

//    image_transport::ImageTransport it(nh);
//    sub1 = it.subscribe(camera_path,1, & vo_system::imgcb,this);

//    odom_pub = nh.advertise<nav_msgs::Odometry>("odom1", 50);

    /// advertising 3D map and camera poses in rviz
//    pub_cloud = nh.advertise<sensor_msgs::PointCloud2> ("dpptam/map", 1);
//    pub_poses = nh.advertise<sensor_msgs::PointCloud2> ("points_poses", 1);
//    vis_pub = nh.advertise<visualization_msgs::Marker>( "dpptam/visualization_marker", 0 );
    /// advertising 3D map and camera poses in rviz


    /// pubishing current frame and the reprojection of the 3D map
//    pub_image = it.advertise("dpptam/camera/image",1);
    /// pubishing current frame and the reprojection of the 3D map


    ros::Time current_time, stamps_ros;
    semidense_tracker.cont_frames = &cont_frames;
    semidense_tracker.stamps = &stamps;
    semidense_tracker.image_frame = &image_frame_aux;
    semidense_tracker.stamps_ros = &stamps_ros;
    ros::Publisher odom_pub;

    ros::Publisher pub_cloud;
    ros::Publisher pub_poses;
    ros::Publisher vis_pub;
    image_transport::Publisher pub_image;
    ///Launch semidense tracker thread*/
    boost::thread thread_semidense_tracker(&ThreadSemiDenseTracker, &images, &semidense_mapper, &semidense_tracker,
                                           &dense_mapper, &Map, &odom_pub, &pub_poses, &vis_pub, &pub_image);

    ///Launch semidense mapper thread
    boost::thread thread_semidense_mapper(&ThreadSemiDenseMapper, &images, &images_previous_keyframe, &semidense_mapper,
                                          &semidense_tracker, &dense_mapper, &Map, &pub_cloud);


    if (calculate_superpixels > 0.5) {
        ///launch dense mapper thread
        boost::thread thread_dense_mapper(&ThreadDenseMapper, &dense_mapper, &pub_cloud);
    }

    cout << "***    DPPTAM is working     *** " << endl << endl;
    cout
            << "***    Launch the example sequences or use your own sequence / "
                    "live camera and update the file 'data.yml' with "
                    "the corresponding camera_path and calibration parameters    ***"
            << endl;

    const int nImages = vstrImageLeft.size();
    for (int i = 0; i < nImages; ++i) {
        // Read left and right images from file

//        ///read images
//        try {
//            cv_bridge::CvImageConstPtr cv_ptr;
//
//            cv_bridge::toCvShare(msg);
//            cv_ptr = cv_bridge::toCvShare(msg);

        stamps = vTimeStamp[i];
        stamps_ros = ros::Time(stamps);
        current_time = stamps_ros;
        image_frame_aux = cv::imread(vstrImageLeft[i], CV_LOAD_IMAGE_UNCHANGED);
//        image_frame_aux = cv_ptr->image.clone();
        cont_frames++;
        semidense_tracker.cont_frames = &cont_frames;
        semidense_tracker.stamps = &stamps;
        semidense_tracker.image_frame = &image_frame_aux;
        semidense_tracker.stamps_ros = &stamps_ros;
        std::cout << "frame :" << i << std::endl;
        sleep(0.1);

    }
    thread_semidense_mapper.join();
    thread_semidense_tracker.join();

}


